//
//  AvoidanceController.cpp
//  symbrion
//
//  Created by Berend Weel on 4/24/13.
//  Copyright (c) 2013 Berend Weel. All rights reserved.
//

#include "AvoidanceController.h"
#include "Mathlib.h"


AvoidanceController::AvoidanceController(WorldModel *worldModel, RobotType *robot) : Controller (worldModel,robot){
    std::cout << "Creating avoidance controller" << std::endl;
}

void AvoidanceController::step(){
    setLEDs();
    double distance = (worldModel->irSensor[0] + worldModel->irSensor[1]) / 2.0;
    double leftSpeed = 1 - ((MAX_SENSOR_RANGE - distance) / MAX_SENSOR_RANGE);
    double rightSpeed = leftSpeed;
                            
    if(distance < 20){
        leftSpeed = rightSpeed = -1;
    }
    
    if (worldModel->irSensor[2] + worldModel->irSensor[3] < worldModel->irSensor[6] + worldModel->irSensor[7]){
        // turn left
        distance = (worldModel->irSensor[2] + worldModel->irSensor[3]) / 2.0;
        leftSpeed -= 0.4 - ((distance / MAX_SENSOR_RANGE) * 0.4);
    }else if (worldModel->irSensor[6] + worldModel->irSensor[7] < 2 * MAX_SENSOR_RANGE){
        // turn right;
        rightSpeed -= 0.4 - ((distance / MAX_SENSOR_RANGE) * 0.4);
    }else{
        if(Mathlib::getBool()){
            leftSpeed -= Mathlib::getRand()*0.2;
        }else{
            rightSpeed -= Mathlib::getRand()*0.2;
        }
    }
    
    worldModel->desiredSpeed[0] = leftSpeed * 10;
    worldModel->desiredSpeed[1] = rightSpeed * 10;
}

void AvoidanceController::setLEDs(){
    for(int i=0;i<robot->getNumDistanceSensors();i++){
        // The mapping from sensor to led is a little weird, don't look :)
        int led1, led2;
        if(i%2 == 0){
            led1 = i*2 + 1;
            led2 = i*2 + 2;
        } else {
            led1 = i*2 + 1;
            led2 = i*2 - 2;
            
        }
        
        if(robot->getDistanceSensorValue(i) < MAX_SENSOR_RANGE - 10){
            worldModel->ledColours.insert(std::pair<int,int>(led1,RED));
            worldModel->ledColours.insert(std::pair<int,int>(led2,RED));
        }else{
            worldModel->ledColours.insert(std::pair<int,int>(led1,GREEN));
            worldModel->ledColours.insert(std::pair<int,int>(led2,GREEN));
        }
    }
}